/***************************************************************************
 *  Filename: controlsender.cpp
 *  Class: ControlSender
 *
 *  Copyright 2012  Mario Henrique Voorsluys
 *  <forcaeluz@gmail.com>
 *
 ****************************************************************************/



#include "controlsender.h"
#include <QDebug>

ControlSender::ControlSender(QObject *parent) :
    QObject(parent)
{
    phi = 0;
    yaw = 0;
    gaz = 0;
    theta = 0;
    sequence = 1;
    abort = false;
    dronestate = 0;
    localaddress = QHostAddress(WIFI_MYKONOS_IP);
    mysocket = new QUdpSocket();
    input = (uint)(1<<ARDRONE_UI_BIT_TRIM_THETA)|
            (1<<ARDRONE_UI_BIT_TRIM_PHI)|
            (1<<ARDRONE_UI_BIT_TRIM_YAW)|
            (1<<ARDRONE_UI_BIT_X)|
            (1<<ARDRONE_UI_BIT_Y);
    QObject::connect(&timer, SIGNAL(timeout()), this, SLOT(startRunning()));
}

void ControlSender::startRunning(){
    qDebug() << "[ControlSender] Spinning once";
    if(abort){
        return;
    }
    mutex.lock();
    int tDronestate = dronestate;
    float tPhi = phi;
    float tTheta = theta;
    float tGaz = gaz;
    float tYaw = yaw;
    mutex.unlock();

    if(tDronestate != 0){
        QByteArray datagram;
        datagram = "";
        if(get_mask_from_state(tDronestate, ARDRONE_COM_WATCHDOG_MASK)
                && !get_mask_from_state(tDronestate, ARDRONE_NAVDATA_BOOTSTRAP)){
            qDebug() << "Reset communication watchdog";
            datagram += "AT*COMWDG=" +QByteArray::number(sequence++) + "\r";
        }else{
            mutex.lock();
            if(emergency || takingoff || landing){
                if(get_mask_from_state(tDronestate, ARDRONE_EMERGENCY_MASK)){
                    input &= ~(1 << ARDRONE_UI_BIT_SELECT);
                    input &= ~(1 << ARDRONE_UI_BIT_START);
                }
                datagram += "AT*REF=" + QByteArray::number(sequence++) + "," + QByteArray::number(input) + "\r";
            }
            mutex.unlock();
            int flag = 0;
            if (tPhi != 0 || tTheta != 0 || tGaz != 0 || tYaw != 0) {
                flag = 1;
            }
            int iroll = *reinterpret_cast<int*>(&tPhi);
            int ipitch = *reinterpret_cast<int*>(&tTheta);
            int igaz = *reinterpret_cast<int*>(&tGaz);
            int iyaw = *reinterpret_cast<int*>(&tYaw);
            datagram += "AT*PCMD=" + QByteArray::number(sequence++)+","+QByteArray::number(flag)
                    +"," + QByteArray::number(iroll)
                    + "," + QByteArray::number(ipitch)
                    + "," + QByteArray::number(igaz)
                    + "," + QByteArray::number(iyaw) +"\r";

        }
        int res=mysocket->writeDatagram(datagram.data(), datagram.size(), localaddress, AT_PORT);
        if(res == -1){
            qDebug() << "[ControlSender] Error sending data";
        }
        qDebug() << "Data Sent: " << datagram;

    }else{
        qDebug() << "[ControlSender] State = 0";
    }
    //QTimer::singleShot(15, this, SLOT(startRunning()));

}

void ControlSender::sendInitSequence(int step)
{
    qDebug() << "Sending init sequence: " << step;

    // A series of commands are send to the drone in order to make it start up in the correct state.
    QByteArray datagram;
    int res;
    if(get_mask_from_state(dronestate, ARDRONE_NAVDATA_BOOTSTRAP) || get_mask_from_state(dronestate, ARDRONE_COMMAND_MASK)){
        switch (step) {

        case 1:
            // We would like to receive a limited amount of feedback data from the drone. We do not need
            // debug data.
            datagram = "AT*CONFIG=" + QByteArray::number(sequence++) + ",\"general:navdata_demo\",\"TRUE\"\r";
            qDebug() << datagram;
            res=mysocket->writeDatagram(datagram.data(),datagram.size(),localaddress,AT_PORT);
            if (res == -1) {
                qDebug()<<"[ControlSender] Error sending navdata_demo config message!";
            }
            break;
        case 2:
            datagram = "AT*CTRL="+QByteArray::number(sequence++) + ",5,0\r";
            res=mysocket->writeDatagram(datagram.data(),datagram.size(),localaddress,AT_PORT);
            qDebug() << datagram;
            if (res == -1) {
                qDebug()<<"[ControlSender] Error sending AT data";
            } else {
                qDebug()<<"[ControlSender] ACK_CONTROL_MODE sent succesfully";
            }

            // This will make the drone set it's offset values. It is important that the drone is standing
            // on a plain surface during startup!
            datagram = "AT*FTRIM="+ QByteArray::number(sequence++)+ "\r";
            res=mysocket->writeDatagram(datagram.data(),datagram.size(),localaddress,AT_PORT);
            if (res == -1) {
                qDebug()<<"[ControlSender] Error sending AT data!!";
            }

            // Enable the right navdata packets;
            quint32 options = NAVDATA_OPTION_MASK(NAVDATA_DEMO_TAG);
            //datagram.clear();
            //datagram = "AT*CONFIG="+QByteArray::number(sequence++) + ",\"general:navdata_options\",\"" + QByteArray::number(options) + "\"\r";
            //res=mysocket->writeDatagram(datagram.data(),datagram.size(),localaddress,AT_PORT);
            //if (res == -1) {
            //    qDebug()<<"[ControlSender] Error sending AT data!!";
            //}
            timer.start(30);
            break;
        }

    }

}

void ControlSender::setemergency(){
    qDebug() << "[ControlSender] EMERGENCY!!!";
    if(!emergency){
        mutex.lock();
        emergency = true;
        input |= (1 << ARDRONE_UI_BIT_SELECT);
        mutex.unlock();
    }

}

void ControlSender::takeoff(){
    qDebug() << "[ControlSender] Take-off";
    mutex.lock();
    takingoff = true;
    input = (1<< ARDRONE_UI_BIT_START);
    mutex.unlock();
}

void ControlSender::updateState(uint state){
    qDebug() << "[ControlSender] Update state";
    mutex.lock();
    dronestate = state;
    mutex.unlock();
}

void ControlSender::updateCommands(float roll, float pitch, float tyaw, float tgaz){
    qDebug() << "[ControlSender] Update commands";
    mutex.lock();
    phi = roll;
    theta = pitch;
    yaw = tyaw;
    gaz = tgaz;
    mutex.unlock();
}

void ControlSender::land(){
    qDebug() << "[ControlSender] land";
    mutex.lock();
    landing = true;
    input &= ~(1<< ARDRONE_UI_BIT_START);
    mutex.unlock();
}

void ControlSender::reset(){
    qDebug() << "[ControlSender] Reset";
    mutex.lock();
    takingoff = false;
    emergency = false;
    input |= (1 << ARDRONE_UI_BIT_SELECT);
    mutex.unlock();
}
